/**
 *****************************************************************************
 * @file    realsense.h
 * @author  JunChao.Yuan, DongKai.Zhou
 * @version V1.2.0-AGV
 * @date    2020.01.03
 * @brief   realsense双目摄像头函数头文件
 *****************************************************************************
 * 版本说明:
 *  V1.0.0 realsense摄像头基本功能例程
 *  V1.1.0 代码规范，完善注释，去除多余函数;探测距离从1.3米改到1.8米
 *  V1.2.0 将KINECT_IMAGE_DISPLAY宏和VIDEO_RECORD_SWITCH变为类内部变量，
 *         新增两变量设置函数
 *******************************************************************************
 * 说明：realsense通过红外传感器生成数据为距离信息的深度图。探测范围：0.3~2.5米
 */

#ifndef VISION_MODULE_REALSENSE_H
#define VISION_MODULE_REALSENSE_H

//foreach和Qt的定义冲突，头文件放在Qt文件之前
#include "librealsense2/rs.hpp"

#include "vision_module/realsense_video.h"
#include "public/error_code.h"
#include "serial/movement_serial_utils.h"
#include "file_utils/file_manager.h"
#include "serial/linux_serial.h"
#include "public/public_thread.h"
#include "public/public_type.h"
#include "vision_module/noise_filter.h"
#include "dcu/dcu_base.h"

#if MAIN_DEBUG_OUTPUT_SWITCH
//调试信息输出开关
#define REALSENSE_DEBUG_OUTPUT    1
#define REALSENSE_CROCESS_THREAD  1
#endif //MAIN_DEBUG_OUTPUT_SWITCH

#include "opencv2/core/core.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/highgui.hpp"
#include "opencv2/video.hpp"
#include "opencv2/videoio.hpp"
#include "OpenNI.h"

#include <QMutex>
#include <QImage>
#include <QTimer>
#include <QList>
#include <math.h>

using cv::Mat;
using cv::Scalar;
using cv::Size;
using cv::Point;
using namespace openni;

/**
 * @brief 双目摄像头控制类2
 */
class RealSense : public PublicThread
{
  Q_OBJECT
  //深度摄像头内参结构
  struct CameraIntrinsicParameters {
    double cx = 325;
    double cy = 197;
    double fx = 476.97;
    double fy = 476.97;
    double scale = 1;
  };

 public:
  /**
   * @brief 获取单例实例
   * @return 第一次调用：新对象指针，其他：当前对象指针
   */
  static RealSense *GetInstance();

  bool CheckDeviceStatus();

  /**
   * @brief 设置数据发送串口序号
   * @param serial_fd：串口描述符序号（同下发串口句柄）
   */
#if (MOVEMENT_PROTOCOL_TYPE == 1)
  void SetSendDataSerial(MovementSerialUtils *serial_fd);
#else //(MOVEMENT_PROTOCOL_TYPE ==  0)
  void SetSendDataSerial(int serial_fd);
#endif //(MOVEMENT_PROTOCOL_TYPE == 1)

  /**
   * @brief 设置视觉图像显示开关标志
   * @param active：标志状态（取值：打开：true，关闭：false）
   * @return 无
   */
  void SetImageDisplayFlag(bool active);

  /**
   * @brief 获取视觉图像显示开关标志
   * @return image_display_flag_
   */
  bool GetImageDisplayFlag();

  /**
   * @brief 设置视觉录像开关标志
   * @param active：标志状态（取值：打开：true，关闭：false）
   * @return 无
   */
  void SetVideoRecordFlag(bool active);

  /**
   * @brief 获取视觉录像开关标志
   * @return video_record_flag_
   */
  bool GetVideoRecordFlag();

  /**
   * @brief 获取设备状态
   * @return dev_status_
   */
  VisionDevStatus GetDevStatus();

  /**
   * @brief 打开设备
   * @return 成功：NO_ERROR，失败：负数错误码
   * @note RealSense驱动安装后会生成对应串口节点文件，程序根据串口序号自动选择设备节点
   */
  int OpenDev();

  /**
   * @brief 关闭设备
   * @return 成功：NO_ERROR，失败：负数错误码
   * @note 主要动作为停止线程，调用关闭语句，更新状态。
   */
  int CloseDev();

  /**
   * @brief 启动设备
   * @return 成功：NO_ERROR，失败：负数错误码
   * @note 调用此函数开启双目摄像头，同时进行深度信息传递和录像
   *       (如果video_record_flag_和obstacle_detect_flag_为ture）
   */
  int StartDev(bool temp_start = false);

  /**
   * @brief 暂停设备
   * @return 成功：NO_ERROR，失败：负数错误码
   * @note 停止双目摄像头功能，但不关闭串口。录像时调用此函数会停止录像并生成录像文件。
   */
  int StopDev();

  /**
   * @brief 允许进行障碍物探测
   * @return 成功：NO_ERROR
   * @note 设置标志位，在线程中检测
   */
  int EnableObstacleDetect();

  /**
   * @brief 禁止进行障碍物探测
   * @return 成功：NO_ERROR
   * @note 设置标志位，在线程中检测
   */
  int DisableObstacleDetect();

  /**
   * @brief 处理深度数据
   * @param mat：摄像头获取到的深度图片
   * @return 成功：NO_ERROR，失败：负数错误码
   * @note 处理深度图和打包数据的主要函数
   */
  int ProcessDepthData(Mat &mat);

  /**
   * @brief 获取区域范围
   * @param index：区域序号（取值：0~kHorizontalZoneNum_）
   * @param x1：左端点横坐标
   * @param x2：右端电横坐标
   * @param y1：上端点纵坐标
   * @param y2：下端电纵坐标
   * @return 成功：NO_ERROR，失败：负数错误码
   * @note 通过此函数获取深度图像各区间的坐标信息
   */
  int GetZoneRange(int index, int *x1, int *x2, int *y1, int *y2);

  /**
   * @brief 获取区域范围
   * @param index：区域序号（取值：0~kHorizontalZoneNum_）
   * @param x1：左端点横坐标
   * @param x2：右端电横坐标
   * @return 成功：NO_ERROR，失败：负数错误码
   * @note 通过此函数获取深度图像各区间的坐标信息
   */
  int GetZoneXRange(int index, int *x1, int *x2);

  /**
   * @brief 获取区域范围
   * @param index：区域序号（取值：0~kVerticalZoneNum_
   * @param y1：上端点纵坐标
   * @param y2：下端电纵坐标
   * @return 成功：NO_ERROR，失败：负数错误码
   * @note 通过此函数获取深度图像各区间的坐标信息
   */
  int GetZoneYRange(int index, int *y1, int *y2);

  /**
   * @brief 获取阈值蒙版
   * @param src：源图像
   * @param dest：蒙版数据生成图像
   * @param threshold：阈值
   * @return 无
   * @note 效果：src图像中数值大于threshold的点dest图像对应点值清零
   */
  void GetThresholdMask(Mat &src, Mat& dest, int threshold_min, int threshold_max);

  /**
   * @brief
   * @param dep_mat：源图像
   * @return 有效的element区域数量
   * @note 效果：过滤深度图噪点
   */
  int FilerDepthNoise(Mat &src, int element_size, double val_prop);

  /**
   * @brief
   * @param dep_mat：源图像
   * @return 无
   * @note 效果：angle值处理
   */
  bool PointAngleTrans(cv::Mat& src, int angle, int height, int height_max);

  /**
   * @brief
   * @param src：源图像
   * @return 检测到楼梯口，返回true
   * @note 效果：楼梯口检测
   */
  bool FallDownObj(Mat &src, int element_size, double val_prop);

  /**
   * @brief
   * @param src：源图像
   * @return 检测双目遮挡，返回true
   * @note 效果：双目遮挡检测
   */
  bool WardCamObj(Mat &dep_src, double inval_pre);

  /**
   * @brief 发送深度信息集合包给底盘
   * @param set：深度信息集合包
   * @return 成功：NO_ERROR，失败：负数错误码
   * @note 按照底盘的通信协议，并获取Movement::write_lock发送深度信息给底盘。
   *       发送间隔：每帧深度图获取并处理完后。
   */
  int SendDepthSet(QVector<DepthInfoPack> &set);

  int DepthToGrayMat(Mat &src, Mat &dest);

  /**
   * @brief 计算校验和
   * @param data：要计算校验和的数据
   * @param n：要计算校验和的数据个数
   * @return 校验和
   */
  uchar CalculateSum(uchar *data, int num);

  /**
   * @brief Mat格式转QImage格式
   * @param frame：Mat格式图片
   * @return 成功：带数据QImage，失败：空QImage
   */
  QImage MatToQImage(Mat &frame);

  /**
   * @brief 获取视觉处理图像
   * @return 处理图像
   * @note 若设备没有启动则可能为空或之前的数据
   */
  QImage GetDepthDisplayImage();

  QImage GetDepthImage();

  /**
   * @brief 获取视觉彩色图像
   * @return 彩色图像
   * @note 若设备没有启动则可能为空或之前的数据
   */
  QImage GetColorImage();

  /**
   * @brief 将视觉彩色图像保存到本地
   * @param path：保存路径
   * @return 成功：NO_ERROR
   */
  int WriteColorMatToDisk(QString path);

  /**
   * @brief 设置双目设备的角度和高度
   * @param angle：角度
   * @param high:高度
   * @return 成功：NO_ERROR
   */
  void SetDeviceParam(int angle,int height,int max_height = 2000);

  /**
   * @brief RealTimeVideoControl 实时推流控制
   * @param on_off 开关
   * @param addr 推流地址
   */
  void RealTimeVideoControl(bool on_off,QString addr);

  /**
   * @brief SetSendDepthData 上层业务控制是否发双目数据到导航
   * @param on_off
   */
  void SetSendDepthData(bool on_off);

 signals:
  /**
   * @brief 录像就绪信号
   * @param file_path：录像文件路径
   * @return 无
   * @note 通知主控类发送录像文件给网络服务器
   */
  void SignalVideoReady(QString file_path, ManagerFileType file_type);

  /**
   * @brief 视觉图像发送信号
   * @return 无
   */
  void SignalDepthImageDisplay();

  /**
   * @brief 视觉线程错误通知
   * @param result：错误码
   * @return 无
   */
  void SignalDeviceResult(int result);

  /**
   * @brief 视觉防跌落状态改变
   * @param fallDown:true:检测到跌落风险,False:告警风险解除
   * @return 无
   */
  void SignalDeviceFallDown(bool fallDown);

  /**
   * @brief SignalRealSenseWorkException
   * @param status 查看对应错误码
   */
  void SignalRealSenseWorkException(int status);

  /**
   * @brief SignalClientCloseRealTimeVideo 摄像头关闭
   */
  void SignalClientCloseRealTimeVideo();

  /**
   * @brief SignalRestartRealsense 检测到残影之后软重启双目
   */
  void SignalRestartRealsense();

  void SignalRealsenseWorkStatus(bool status);

  void SignalTransmitDataToDcu(uchar cmad, int flame_type, QByteArray data);

  //orbeec camera test
  int OpenDeviceOrbbec();
  int CloseDeviceOrbbec();
  void GetCameraParamsOrbbec();
  int StartProcessOrbbec(cv::Mat &depth_mat);

 protected:
  /**
   * @brief 调试输出函数
   * @param text：输出内容
   * @return 无
   * @note 方便输出信息集中管理
   */
  void DebugOutput(const QString &text, int level = 1);


  /**
   * @brief 图像处理线程
   * @return 无
   * @note 在此线程中完成图像获取，深度数据整理发送和录像功能。
   */
  void run();

protected slots:
  /**
   * @brief 临时开启摄像头超时处理
   * @return 无
   */
  void TempStartTimeout();

  /**
   * @brief RestartRealsenseTimeout 重启流程
   */
  void RestartRealsenseTimeout();

  void SlotRealsenseReceiveTimeout();

private:
  /**
   * @brief 构造函数
   */
  RealSense();

  /**
   * @brief 类析构函数
   */
  ~RealSense();

  /**
   * @brief CheckRealSenseWorkStatus 检测双目工作状态
   */
  void CheckRealSenseWorkStatus();

  /**
   * @brief StartRealSenseDataFlow 开启数据流
   * @return
   */
  int StartRealSenseDataFlow();

  /**
   * @brief RestartRealsense 双目电源控制
   * @param on_off 0:下电 1:上电
   */
  int RealsensePowerControl(uchar on_off);

  /**
    * @brief CheckGhostInit检测初始化
    */
   void CheckGhostInit();

  /**
   * @brief CheckGhostImage 检测残影
   * @param mat
   * @return
   */
  bool CheckGhostImage(cv::Mat mat);

  /**
   * @brief RealSenseWorkExceptionDeal  realsense 出现异常后的处理
   * @param error 错误
   */
  void RealSenseWorkExceptionDeal(int error);


  //双目深度摄像头info
  CameraIntrinsicParameters camera_info;
  //双目深度摄像头最大探测范围设置（单位：cm，最大250）
  const uchar kMaxDetectDistance_ = 250;
  //双目深度摄像头最小探测范围（单位：cm，最小0）
  const uchar kMinDetectDistance_ = 20;
  //颜色划分的距离依据（单位：cm，最大250）
  const uchar kWarnDistance_      = 100;
  //限定彩色图片宽度
  const int kColorFrameWidth_   = 640;
  //限定彩色图片高度
  const int kColorFrameHeight_  = 480;
  //限定深度图片宽度
  const int kDepthFrameWidth_   = 640;
  //限定深度图片高度
  const int kDepthFrameHeight_  = 400;

  //X轴划分区间的个数
  const int kHorizontalZoneNum_ = 32;
  //Y轴划分区间的个数
  const int kVerticalZoneNum_ = 3;
  //多少帧保存一次视频
  const uint save_video_frame_count_ = 12000;
  //浮点型0认定界线
  const float kFloatMinimum_    = float(0.00001);

  //Y轴上方屏蔽部分比例（不统计数据比例，0为不屏蔽）
#if (ROBOT_TYPE_SELECT == 5)
  const float kYTopIgnoreRate_    = 0;
#else //(ROBOT_TYPE_SELECT != 5)
  const float kYTopIgnoreRate_    = 0;
#endif //(ROBOT_TYPE_SELECT == 5)

  //Y轴下方屏蔽部分比例（不统计数据比例，0为不屏蔽）
#if (ROBOT_TYPE_SELECT == 5)
  const float kYBotIgnoreRate_    = 1.0/8;
#else //(ROBOT_TYPE_SELECT != 5)
  const float kYBotIgnoreRate_    = 0;
#endif //(ROBOT_TYPE_SELECT == 5)
  //圆周率常量
  const double kPi_             = 3.141592653;
  //预测的每秒帧数
  const double kFrameFpsIndict_   = 15;

  //视觉图像显示开关标志
  bool image_display_flag_ = false;
  //用于测试
  bool image_display_flag_keep_ = false;
  //视觉录像开关标志
  bool video_record_flag_ = true;

  //障碍物检测开启标志
  bool obstacle_detect_flag_ = true;
  //处理数据时x坐标抽样值(抽样越大越节省时间，但精度越低)
  int x_sample_val_ = 4;
  //处理数据时y坐标抽样值(同步修改SetImageDisplayFlag函数值)
  int y_sample_val_ = 4;
  //设备状态
  VisionDevStatus dev_status_ = kVisionDevClosed;
#if !REALSENSE_CROCESS_THREAD
  //双目设备数据管道
  rs2::pipeline pipe_;
  //双目设备配置信息
  rs2::config cfg_;
#endif
  //深度信息集合包
  QVector<DepthInfoPack> depth_info_set_;
  //视觉处理图像
  Mat display_mat_;
  //视觉处理图像输出
  QImage display_image_;
  //获取处理图像锁
  QMutex display_image_lock_;

  //彩色图像
  Mat color_mat_;
  //彩色图像输出
  QImage color_image_;
  //获取彩色图像锁
  QMutex color_image_lock_;

  //深度图像
  Mat depth_mat_;
  //深度图像输出
  QImage depth_image_;
  //获取深度图像锁
  QMutex depth_image_lock_;

  //临时开启双目定时器（服务器拍照功能用）
  QTimer temp_start_timer_;
  //单位ms
  int temp_start_timeout_val_ = 180000;
  //单例对象指针
  static RealSense *instance_;
#if !REALSENSE_CROCESS_THREAD
  //用于工作状态检查
  rs2::pipeline_profile m_profile_;
#endif //!REALSENSE_CROCESS_THREAD
  //限制功率
  float m_power_limited_ = 0.0;
  //限制温度
  float m_temper_limited_ = 0.0;
  //限制丢包率,百分比
  int m_lost_packet_ = 0;
  //上报的工作状态
  int m_upload_work_status_ = -1;

  //双目工作功率衰减，低于这个重启双目,配置文件读取
  int m_vision_module_power_limited = 0;
  //临时策略的开关，配置文件读取
  int m_restart_realsense_switch_ = 0;
#if !REALSENSE_CROCESS_THREAD
  //写视频文件
  VideoSaveClient m_video_save_;
#else
  RealsenseVideo realsense_video_;
#endif
  //定时软重启，解决残影问题
  QTimer m_restart_realsense_timer_;

  //接收双目超时计时器
  QTimer m_realsense_receive_timer_;

  //软重启时间间隔
  int m_restart_realsense_time_ = 0;
  //写视频文件时候是否新建(定时重启不需要新建文件)
  bool m_write_new_video_file_ = true;
  //检测图片相似性的实例
  NoiseFilter m_check_similar_;
  //缓存图像帧
  cv::Mat m_color_flame_[25];

  DcuBase *dcu_base_ = nullptr;
  //是否发送数据到导航
  bool send_depth_data_ = true;
  bool m_realsense_work_status = true;
  bool m_realsense_last_work_status = true;

  //orbeec camera test
  openni::Device device;
  VideoStream depth;
  VideoFrameRef frame;
  openni::Status stat_;

  //双目角度
#if (ROBOT_TYPE_SELECT >= 5)
  //E1
  //双目角度（向上为负数）
  int device_angle = 0;
  //双目离地高度（单位mm）
  int device_height = 865;
  //机器人最大高度(原measure_height)
  int robot_max_height = 2000;
#elif (ROBOT_TYPE_SELECT == 4)
#if (ROBOT_CLASS_SELECT == 2)
  //B2
  int device_angle = 45;
  int device_height = 940;
  int robot_max_height = 1340;
#else //(ROBOT_CLASS_SELECT != 2)
  //B1
  int device_angle = 0;
  int device_height = 1000;
  int robot_max_height = 1250;
#endif //(ROBOT_CLASS_SELECT == 2)
#elif (ROBOT_TYPE_SELECT == 3)
  //A1
  int device_angle = 0;
  int device_height = 850;
  int robot_max_height = 1500;
#elif (ROBOT_TYPE_SELECT == 2)
  //智能柜和传输装置无双目
  int device_angle = 0;
  int device_height = 0;
  int robot_max_height = 0;
#elif (ROBOT_TYPE_SELECT <= 1)
  //C2
#if (ROBOT_CLASS_SELECT == 1)
  int device_angle = 0;
  int device_height = 785;
  int robot_max_height = 1600;
#else //(ROBOT_CLASS_SELECT == 2)
  int device_angle = 45;
  int device_height = 1030;
  int robot_max_height = 1600;
#endif //(ROBOT_CLASS_SELECT == 1)
#endif //(ROBOT_TYPE_SELECT >= 5)

  bool fall_down_status_ = false;

#if (MOVEMENT_PROTOCOL_TYPE == 1)
  MovementSerialUtils* serial_utils_ = nullptr;
#else //(MOVEMENT_PROTOCOL_TYPE ==  0)
  //底盘串口句柄保存
  int dev_handle_ = -1;
#endif //(MOVEMENT_PROTOCOL_TYPE == 1)
};

#endif //VISION_MODULE_REALSENSE_H
